#include "item.h"

using namespace std;

Item::Item(double _K, double _gamma, double _n0, double _lambda, double _k_r_t)
    : K(_K)
    , gamma(_gamma)
    , n0(_n0)
    , lambda(_lambda)
    , k_r_t(_k_r_t)
{
    t_detection.sec = INF;
}

Vector3d Item::calculateForce()
{
    // 超过n0距离的就不参与计算了
    if ( position.norm() > n0 ) {
        return Vector3d(0, 0, 0);
    }

    // 计算参数值
    double   speed           = get_speed();
    double   k_r_i           = get_K();
    double   dis_coefficient = get_dis_coefficient();
    Vector3d theta_i_n       = get_theta();

#if DEBUG
    ROS_INFO("speed : %f  |   k_r_i : %f   |   dis_coefficient : %f\n", speed, k_r_i, dis_coefficient);
    cout << theta_i_n << endl;
#endif

    // 如果k_r_i过于小，说明很长时间没有检测到这个东西了，把这个东西去掉
    if ( k_r_i < k_r_t ) {
        return Vector3d(INF, INF, INF);
    }

    return speed * k_r_i * dis_coefficient * theta_i_n;
}

double Item::get_dis_coefficient()
{
    double ni = position.norm();
    return 1 - ((1 - exp(gamma * ni)) / (1 - exp(gamma * n0)));
}

Vector3d Item::get_theta()
{
    // 计算球的速度与位置的法向量，并且确保法向量朝上方
    Vector3d theta_i = v.cross(position);
    if ( theta_i(2) < 0 ) theta_i = -theta_i;

    // TODO
    // 这里对无人机航向角暂不计算
    // Vector3d theta_i_n = theta_i - theta_i.dot(theta_quadrotor) * theta_quadrotor;

    return theta_i;
}

double Item::get_K()
{
    if ( t_detection.sec == INF ) {
        t_detection = ros::Time::now();
    }

    ros::Time now  = ros::Time::now();
    double    diff = (t_detection - now).toSec();
    double    ans  = K * exp(-lambda * (t_detection - now).toSec());
    t_detection    = now;
    return ans;
}

Items::Items(ros::NodeHandle& nh, double _K, double _gamma, double _n0, double _lambda, double _k_r_t, double _F)
    : K(_K)
    , gamma(_gamma)
    , n0(_n0)
    , lambda(_lambda)
    , k_r_t(_k_r_t)
    , F(_F)
{
    force_pub = nh.advertise<std_msgs::Float64MultiArray>("/force", 1);
}

void Items::feed(const std_msgs::Float64MultiArrayConstPtr& msg)
{
    int    cnt = 0;
    double n   = msg->data[cnt++];
    // TODO
    // 校验msg

    for ( int i = 0; i < n; i++ ) {
        int id = msg->data[cnt++];
        // 没找到这个id里面的item
        if ( data.find(id) == data.end() ) {
            Item item(K, gamma, n0, lambda, k_r_t);
            data[id] = item;
        }
        data[id].v(0)        = msg->data[cnt++];
        data[id].v(1)        = msg->data[cnt++];
        data[id].v(2)        = msg->data[cnt++];
        data[id].position(0) = msg->data[cnt++];
        data[id].position(1) = msg->data[cnt++];
        data[id].position(2) = msg->data[cnt++];
    }

    calculate();
}

void Items::calculate()
{
    force = {0, 0, 0};

    auto it = data.begin();
    while ( it != data.end() ) {
        Item     item = it->second;
        Vector3d f    = item.calculateForce();

        // 如果计算返回了INF，说明已经很长时间没有检测到这个东西了，删去
        if ( f(0) == INF ) {
            it = data.erase(it);
        }
        else {
            force += f;
            ++it;
        }
    }

    std_msgs::Float64MultiArray ans;

    ans.data.push_back(1);
    ans.data.push_back(force(0));
    ans.data.push_back(force(1));
    ans.data.push_back(force(2));

    force_pub.publish(ans);
}

